
//! 主算法逻辑
#include "bsp.h"

// 定义CAN数据包结构体
typedef struct {
	 short oneselfWithCaptainDiff;  // 队长与自己的差值
	 uint16_t queueMember;             // 队列成员
	 uint16_t GetSetFrequency;         // 设置频率
	 uint16_t nGetRunFrequency;        // 运行频率
} CanSyncData_t;

// 定义CAN ID位掩码
#define CAN_ID_CARDID_MASK  0x03  // 位0-1: 板卡ID
#define CAN_ID_CAPTAIN_FLAG 0x04  // 位2: 队长标志
#define CAN_ID_SPEED_LOSS   0x20  // 位5: 失速标志
#define CAN_ID_SYNC         0x10  // 位4: 同步标志

 
// 常量定义
#define CAPTAIN_FLAG            0x04  // 队长标志
#define SYNC_FLAG               0x08  // 同步标志
#define DEAD_TIME               5     // 死区时间
#define SCALE_FACTOR_K          1.0   // 比例系数
#define MAX_TIME_LOOP           30    // 最大时间循环
#define DIFFERENCE_THRESHOLD    100   // 差值阈值
#define CO_CHANNEL_THRESHOLD    10    // 同频阈值
#define CAPTAIN_CO_CHANNEL_THRESHOLD 10 // 队长同频阈值

#define TIMER_PERIOD            pdMS_TO_TICKS(1000) // 定时器周期
#define TIMER_VPTSET            17500  // 定时器阈值，单位ms
#define INTERVAL_TIMEOUT        10000  // 10秒超时
#define CO_CHANNEL_COMPLETE     0x01   // 同频完成标志
#define CO_CHANNEL_FAIL         0x02   // 同频失败标志
 
static void adjustFrequencyToCaptain(unsigned short setFreq, unsigned short captainFreq, unsigned short threshold)
{
	 if (setFreq > captainFreq)
	 {
			 unsigned short difference = setFreq - captainFreq;
			 if (difference >= threshold)
			 {
					 if (UivertorRefreshData(captainFreq) == 0)
					 {
							 xEventGroupSetBits(xFrequencyUpdateLCDCaptain, 0x01);
					 }
			 }
			 else
			 {
					 SEGGER_RTT_CustomFunction(2, 0, "Difference allowed: %d\r\n", difference);
			 }
	 }
	 else if (setFreq < captainFreq)
	 {
			 unsigned short difference = captainFreq - setFreq;
			 if (difference >= threshold)
			 {
					 if (UivertorRefreshData(captainFreq) == 0)
					 {
							 xEventGroupSetBits(xFrequencyUpdateLCDCaptain, 0x01);
					 }
					 xSemaphoreTake(xMutex, portMAX_DELAY);
					 g_SetFrequencyUpdateValue = captainFreq;
					 xSemaphoreGive(xMutex);
			 }
			 else
			 {
					 SEGGER_RTT_CustomFunction(2, 0, "Difference allowed: %d\r\n", difference);
			 }
	 }
	 else
	 {
			 SEGGER_RTT_CustomFunction(2, 0, "Frequencies equal\r\n");
	 }
} 
 
///**
// * @brief 发送CAN同步信息
// * @param SetCardId 板卡ID (0-3)
// * @param queueMember 队列成员
// * @param GetSetFrequency 设置频率
// * @param nGetRunFrequency 运行频率
// * @param oneselfWithCaptainDiff 队长与自己的差值
// * @param CaptainSign 队长标志 (0: 不是队长, 1: 是队长)
// * @param speedLoss 失速标志 (0: 正常, 1: 失速)
// * @param sync 同步标志 (0: 未同步, 1: 已同步)
// */
//void CanSyncMessage(uint8_t SetCardId,
//                    uint16_t queueMember,
//                    uint16_t GetSetFrequency,
//                    uint16_t nGetRunFrequency,
//                    short oneselfWithCaptainDiff,
//                    uint8_t CaptainSign,
//                    uint8_t speedLoss,
//                    uint8_t sync)
//{
//    // 构建CAN ID
//    uint8_t CanSyncID = SetCardId & CAN_ID_CARDID_MASK;  // 板卡ID (位0-1)

//    if (CaptainSign) {
//        CanSyncID |= CAN_ID_CAPTAIN_FLAG;  // 设置队长标志 (位2)
//    }
//    if (speedLoss) {
//        CanSyncID |= CAN_ID_SPEED_LOSS;    // 设置失速标志 (位5)
//    }
//    if (sync) {
//        CanSyncID |= CAN_ID_SYNC;          // 设置同步标志 (位4)
//    }
//    
//    #pragma pack(1) // 确保没有填充字节
//    // 构建CAN数据包
//    CanSyncData_t data = {
//        .oneselfWithCaptainDiff = oneselfWithCaptainDiff,
//        .queueMember = queueMember,
//        .GetSetFrequency = GetSetFrequency,
//        .nGetRunFrequency = nGetRunFrequency
//    };
//    #pragma pack() // 恢复默认对齐
//    
//    // 准备CAN发送数据
//    uint8_t CanSyncData[8];
//    memcpy(CanSyncData, &data, sizeof(CanSyncData_t));

//    // 设置CAN头部
//    CanTxHeadType.StdId = CanSyncID;
//    // 发送CAN消息并进行错误处理
//    if (CAN_Send_Mesage(CAN1_Handler, &CanTxHeadType, &CanSyncData) != 0) {
//        // 错误处理: 记录日志或采取其他措施
//    }
//}

///**
// * @brief 通过CAN下发频率调整信息
// * @param num 板卡编号
// * @param CaptainSign 是否为队长
// * @param Frequency 频率的具体信息
// * @return 0 表示成功
// */
//unsigned char CanSendFrequencyAdjustment(unsigned char num, unsigned char CaptainSign, unsigned short Frequency)
//{
//    unsigned short CanSyncID = num;
//    if (CaptainSign == 1)
//        CanSyncID |= CAPTAIN_FLAG;
//    CanSyncID |= SYNC_FLAG;
//    static unsigned char CanSyncData[8] = {0};
//    CanSyncData[0] = num;
//    CanSyncData[1] = Frequency >> 8;
//    CanSyncData[2] = Frequency; 
//    CanTxHeadType.StdId = CanSyncID;
//    CAN_Send_Mesage(CAN1_Handler, &CanTxHeadType, &CanSyncData);    
//    return 0;
//}

///**
// * @brief 变频器更新数据
// * @param setNewFrequency 变频器的更新频率
// * @return 0 表示成功，-1 表示失败
// */
//char UivertorRefreshData(unsigned short setNewFrequency)
//{
//    static char nerror = 0;
//    static INVERTERCOMMAND SetCommand = {0x0, 0x0, 0x0, 0x0, 0x0};  // 给变频器发送的指令
//    SetCommand.InverterAddress = DRIVE_ID;  // 变频器ID，需要注意，如果不是1需更改宏定义
//    SetCommand.Command = 0x06;              // 写命令
//    SetCommand.ParameterAddress = swap_uint16(0x2001);  // 参数地址
//    SetCommand.ParameterData = swap_uint16(setNewFrequency);  // 频率
//    SetCommand.CrcCheck = crc16_modbus((uint8_t *)&SetCommand, 6);  // 计算CRC
//    if (xQueueSend(MotorRs485Queuesend, &SetCommand, NULL) != pdPASS) {  // 队列入队
//        SEGGER_RTT_CustomFunction(1, 0, "MotorRs485Queuesend is error\r\n");
//        nerror = -1;                                
//    } else {
//        nerror = 0;
//    }
//    return nerror;
//}

///**
// * @brief 计算同步频率，与队长的距离差≤1个节拍
// * @param oneselfCycle 自己的周期值ms
// * @param oneselfWithCaptainDiff 自己的周期与队长的周期差值
// * @param CaptainFrequency 队长的固定周期
// * @return 新设置的频率，0表示无需调整
// */
//unsigned short synchronizingFrequency(int oneselfCycle, int oneselfWithCaptainDiff, int CaptainFrequency)
//{
//    static int Distance = 0;  // 距离差值
//    int Offset = 0;           // 补偿值
//    int deadTime = DEAD_TIME; // 死区时间
//    double scaleFactorK = SCALE_FACTOR_K; // 比例系数

//    static unsigned short setNewnGetCaptainSetFrequency = 0;  // 给变频器输入的新频率
//    unsigned short nSetNewFrequency = 0;

//    if (oneselfWithCaptainDiff <= oneselfCycle * 1) {
//        // 计算距离差值
//        if (oneselfWithCaptainDiff > (oneselfCycle / 2)) {
//            Distance = oneselfWithCaptainDiff - oneselfCycle;
//        } else if (oneselfWithCaptainDiff < (oneselfCycle / 2)) {
//            Distance = oneselfWithCaptainDiff;
//        } else {
//            // 差值等于一半周期，无需特殊处理
//        }
//        g_Distance = Distance; // 更新全局实际差值

//        if (abs(oneselfWithCaptainDiff) > deadTime) {  // 超过死区时间，需要补偿
//            Offset = (double)(Distance * scaleFactorK);  // 计算补偿值
//            setNewnGetCaptainSetFrequency = CaptainFrequency + Offset;  // 队长频率加上补偿
//            nSetNewFrequency = setNewnGetCaptainSetFrequency;
//            SEGGER_RTT_CustomFunction(5, 0, "setNewnGetCaptainSetFrequency = %d\r\n", setNewnGetCaptainSetFrequency);
//        } else {  // 无需补偿
//            SEGGER_RTT_CustomFunction(5, 0, "Within dead time: %d <= %d\r\n", abs(oneselfWithCaptainDiff), deadTime);
//        }
//    }
//    return nSetNewFrequency;
//}

///**
// * @brief 处理成员同步逻辑
// * @param memberNum 成员编号
// * @param diff 成员与队长的差值
// */
//static void handleMemberSync(unsigned char memberNum, short diff) {
//    static unsigned timeLoop[4] = {0, 0, 0, 0};  // 成员同步计数器数组，初始值全为0
//    if (memberNum >= 4) return;  // 超出范围，返回
//    if (timeLoop[memberNum] <= MAX_TIME_LOOP) {
//        timeLoop[memberNum]++;  // 计数增加，跟踪同步尝试次数
//    } else if (timeLoop[memberNum] > MAX_TIME_LOOP) {
//        if (CardId != memberNum) {  // 不是当前板卡，可能是其他成员
//            timeLoop[memberNum] = 0;  // 重置计数器
//            // TODO: 实现成员队列移除逻辑，例如：removeMemberFromQueue(memberNum);
//        }
//    }
//    if (abs(diff) <= DIFFERENCE_THRESHOLD) {  // 差值小于阈值，同步完成
//        timeLoop[memberNum] = 0;  // 重置计数器
//    }
//    // 显示逻辑：负差值显示为负，正差值显示为正
//    if (diff < 0) {
//        displayQueueMessage(memberNum, 1, abs(diff) / 100);  // 负差值
//    } else {
//        displayQueueMessage(memberNum, 0, diff / 100);  // 正差值
//    }
//}

///**
// * @brief 检查频率同步状态并处理超时
// * @param setFreq 设置频率
// * @param runFreq 运行频率
// * @param threshold 频率差值阈值
// */
//static void checkFrequencySynchronization(unsigned short setFreq, unsigned short runFreq, unsigned short threshold) {
//    int difference = (int)setFreq - (int)runFreq;  // 计算差值，考虑符号
//    unsigned short absDifference = abs(difference);  // 取绝对值

//    if (absDifference <= threshold) {
//        // 频率差值在阈值内，同步完成
//        if (xTimerIsTimerActive(SpeedTimers) == pdTRUE) {
//            xTimerStop(SpeedTimers, 0);  // 停止定时器
//        }
//        SEGGER_RTT_CustomFunction(2, 0, "Co_ChannelBits is complete\r\n");
//        xEventGroupClearBits(xCo_ChannelEventGroup, CO_CHANNEL_FAIL);
//        xEventGroupSetBits(xCo_ChannelEventGroup, CO_CHANNEL_COMPLETE);
//    } else {
//        // 频率差值超过阈值，启动定时器
//        if (xTimerIsTimerActive(SpeedTimers) == pdFALSE) {
//            xTimerStart(SpeedTimers, TIMER_PERIOD);  // 启动定时器
//        }
//        // 检查定时器是否超时
//        if (g_SpeedTimeLoop >= TIMER_VPTSET) {
//            xTimerStop(SpeedTimers, TIMER_PERIOD);
//            SEGGER_RTT_CustomFunction(5, 0, "stall is error\r\n");  // 记录失速错误
//            xEventGroupClearBits(xCo_ChannelEventGroup, CO_CHANNEL_COMPLETE);
//            xEventGroupSetBits(xCo_ChannelEventGroup, CO_CHANNEL_FAIL);  // 设置同频失败
//        } else {
//            SEGGER_RTT_CustomFunction(2, 0, "g_SpeedTimeLoop = %d\r\n", g_SpeedTimeLoop);  // 记录同步过程中计时
//        }
//    }
//}

///**
// * @brief 获取当前频率值
// * @param setFreq 指向设置频率的指针
// * @param runFreq 指向运行频率的指针
// * @param captainSetFreq 指向队长设置频率的指针
// */
//static void getFrequencies(unsigned short *setFreq, unsigned short *runFreq, unsigned short *captainSetFreq) {
//    xSemaphoreTake(xMutex, portMAX_DELAY);
//    *setFreq = g_SetFrequencyUpdateValue;
//    *runFreq = g_RunFrequencyUpdateValue;
//    *captainSetFreq = g_CaptainSetFrequencyUpdateValue;
//    xSemaphoreGive(xMutex);
//}

///**
// * @brief 设置自己为队长
// * @param setFreq 当前设置的频率
// */
//static void electSelfAsCaptain(unsigned short setFreq) {
//    BOTTLING SetBottling = {CardId, setFreq, 1, 0};  // 设置为队长
//    displayLeader(1);  // 显示为队长状态
//    xEventGroupSetBits(xCaptainExistEventGroup, 0x01);  // 设置队长存在标志
//    if (xQueueSend(BottlingQueuesend, &SetBottling, NULL) != pdPASS) {
//        SEGGER_RTT_CustomFunction(5, 0, "Bottling queue send error\r\n");
//    }
//}

///**
// * @brief 检查是否为队长
// * @return true 如果是队长，false 否则
// */
//static bool isSelfCaptain() {
//    BOTTLING SpyOn;
//    if (xQueuePeek(BottlingQueuesend, &SpyOn, pdMS_TO_TICKS(500)) == pdPASS) {
//        return (SpyOn.CardNum == CardId && SpyOn.CaptainSign == 1);
//    }
//    return false;
//}

///**
// * @brief 处理队长逻辑
// */
//static void handleCaptainLogic() {
//    if (g_interval_ms > INTERVAL_TIMEOUT) {
//        BOTTLING TakeOut;
//        xQueueReceive(BottlingQueuesend, &TakeOut, 0);  // 移除队列数据
//        BOTTLING resign = {CardId, 0, 0, 0};  // 放弃队长身份
//        xQueueSend(BottlingQueuesend, &resign, NULL);  // 发送放弃消息
//        xEventGroupClearBits(xCaptainExistEventGroup, 0x01);  // 清除队长存在标志
//        displayLeader(0);  // 显示为队员
//    } else {
//        LCDShowQueue member;
//        if (xQueueReceive(MemberMessag, &member, pdMS_TO_TICKS(10)) == pdPASS) {
//            handleMemberSync(member.memberNum, member.memberWithCaptainDiff);  // 处理成员同步
//        } else {
//            SEGGER_RTT_CustomFunction(5, 0, "MemberMessag read error\r\n");
//        }
//    }
//}

///**
// * @brief 处理队员逻辑
// */
//static void handleMemberLogic() {
//    unsigned short setNewFrequency = synchronizingFrequency(g_interval_ms, g_CaptainSyncDIFISR, nGetCaptainSetFrequency);
//    if (setNewFrequency != 0) {
//        if (UivertorRefreshData(setNewFrequency) == 0) {
//            SEGGER_RTT_CustomFunction(5, 0, "setNewFrequency = %d\r\n", setNewFrequency);
//            xEventGroupSetBits(xFrequencyUpdateLCDCaptain, 0x01);  // 更新LCD频率
//        }
//    } else if (g_CaptainSyncDIFISR > g_interval_ms * 1) {
//        SEGGER_RTT_CustomFunction(5, 0, RTT_CTRL_TEXT_BRIGHT_RED "Excessive deviation value\r\n");
//    }
//    // 显示差值
//    if (g_Distance >= 0) {
//        displayQueueMemberMessage(CardId, 0, abs(g_Distance) / 100);
//    } else {
//        displayQueueMemberMessage(CardId, 1, abs(g_Distance) / 100);
//    }
//}

///**
// * @brief 处理非同步模式
// */
//static void handleNonSyncMode() {
//    BOTTLING SetBottling = {CardId, 0, 0, 0};  // 放弃队长身份
//    if (CardId <= 3) {
//        CanSyncMessage(CardId, 0x1234, nGetSetFrequency, nGetRunFrequency, g_Distance, 0, 0, 0);
//    }
//    xEventGroupClearBits(xCaptainExistEventGroup, 0x01);
//    xEventGroupClearBits(xCo_ChannelEventGroup, CO_CHANNEL_COMPLETE | CO_CHANNEL_FAIL);
//    SEGGER_RTT_CustomFunction(5, 0, "Non-sync mode: cleared flags\r\n");
//}

///**
// * @brief 同步逻辑任务
// * @param argument 任务参数（未使用）
// */
//void SyncLogicTask(void *argument) {
//    const TickType_t xTicksToWait = portMAX_DELAY; // 阻塞等待，提高响应性
//    while (1) {
//        EventBits_t PulseBits = xEventGroupWaitBits(xPulseEventGroup, 0x01, pdTRUE, pdFALSE, xTicksToWait);
//        if (PulseBits & 0x01) {  // 脉冲有效
//            EventBits_t SyncBits = xEventGroupGetBits(xSyncEventGroup);
//            if (SyncBits & 0x01) {  // 同步模式
//                EventBits_t Co_ChannelBits = xEventGroupGetBits(xCo_ChannelEventGroup);
//                EventBits_t CaptainExistBits = xEventGroupGetBits(xCaptainExistEventGroup);
//                unsigned short setFreq, runFreq, captainSetFreq;
//                getFrequencies(&setFreq, &runFreq, &captainSetFreq);

//                if (Co_ChannelBits == 0) {
//                    if (CaptainExistBits == 1) {
//                        adjustFrequencyToCaptain(setFreq, captainSetFreq, CAPTAIN_CO_CHANNEL_THRESHOLD);
//                    }
//                    checkFrequencySynchronization(setFreq, runFreq, CO_CHANNEL_THRESHOLD);
//                } else if (Co_ChannelBits == 1) {
//                    if (CaptainExistBits == 0) {
//                        electSelfAsCaptain(setFreq);
//                    }
//                    if (CardId <= 3) {
//                        CanSyncMessage(CardId, 0x1234, setFreq, runFreq, g_CaptainSyncDIFISR, isSelfCaptain() ? 1 : 0, 0, 1);
//                    }
//                    if (isSelfCaptain()) {
//                        handleCaptainLogic();
//                    } else {
//                        handleMemberLogic();
//                    }
//                } else if (Co_ChannelBits == 2) {
//                    SEGGER_RTT_CustomFunction(5, 0, RTT_CTRL_TEXT_BRIGHT_YELLOW "co-Channel fail Stall alarm\r\n");
//                } else {
//                    SEGGER_RTT_CustomFunction(5, 0, RTT_CTRL_TEXT_BRIGHT_RED "co-Channel fail unknown error\r\n");
//                }
//            } else if (SyncBits & 0x02) {  // 非同步模式
//                handleNonSyncMode();
//            }
//        }
//    }
//} 
 
/*******/ 
 
/// @brief 发送CAN同步信息
/// @param SetCardId           板卡ID (0-3)
/// @param queueMember         队列成员
/// @param GetSetFrequency     设置频率
/// @param nGetRunFrequency    运行频率
/// @param oneselfWithCaptainDiff  队长与自己的差值
/// @param CaptainSign         队长标志 (0: 不是队长, 1: 是队长)
/// @param speedLoss           失速标志 (0: 正常, 1: 失速)
/// @param sync                同步标志 (0: 未同步, 1: 已同步)
void CanSyncMessage(uint8_t SetCardId,
									 uint16_t queueMember,
									 uint16_t GetSetFrequency,
									 uint16_t nGetRunFrequency,
									 short oneselfWithCaptainDiff,
									 uint8_t CaptainSign,
									 uint8_t speedLoss,
									 uint8_t sync)
{
	 // 构建CAN ID
	 uint8_t CanSyncID = SetCardId & CAN_ID_CARDID_MASK;  // 板卡ID (位0-1)

	 if (CaptainSign) {
			 CanSyncID |= CAN_ID_CAPTAIN_FLAG;  // 设置队长标志 (位2)
	 }
	 if (speedLoss) {
			 CanSyncID |= CAN_ID_SPEED_LOSS;    // 设置失速标志 (位5)
	 }
	 if (sync) {
			 CanSyncID |= CAN_ID_SYNC;          // 设置同步标志 (位4)
	 }
	
	#pragma pack(1) // 确保没有填充字节
	 // 构建CAN数据包
	 CanSyncData_t data = {
			 .oneselfWithCaptainDiff = oneselfWithCaptainDiff,
			 .queueMember = queueMember,
			 .GetSetFrequency = GetSetFrequency,
			 .nGetRunFrequency = nGetRunFrequency
	 };
	#pragma pack() // 恢复默认对齐
	
	 // 准备CAN发送数据
	 uint8_t CanSyncData[8];
	 memcpy(CanSyncData, &data, sizeof(CanSyncData_t));

	 // 设置CAN头部
	 CanTxHeadType.StdId = CanSyncID;
	 // 发送CAN消息并进行错误处理
	 if (CAN_Send_Mesage(CAN1_Handler, &CanTxHeadType, &CanSyncData) != 0) {
			 // 错误处理: 记录日志或采取其他措施
	 }
}

/// @brief 通过CAN下发频率调整信息
/// @param num 			板卡编号
/// @param CaptainSign 	是否为队长
/// @param Frequency 	频率的具体信息
/// @return 
unsigned char CanSendFrequencyAdjustment(unsigned char num , unsigned char CaptainSign , unsigned short Frequency)
{
	unsigned short CanSyncID = num;
	if(CaptainSign == 1)
		CanSyncID = num | 0x4;
	CanSyncID = CanSyncID | 0x8;
	static unsigned char  CanSyncData[8] = {0};
	CanSyncData[0] = num;
	CanSyncData[1] = Frequency >> 8;
	CanSyncData[2] = Frequency; 
	CanTxHeadType.StdId = CanSyncID;
	CAN_Send_Mesage(CAN1_Handler , &CanTxHeadType , &CanSyncData);	
	return 0;
}

/// @brief 变频器更新数据
/// @param setNewFrequency 变频器的更新频率
char UivertorRefreshData(unsigned short setNewFrequency)
{
	static char nerror = 0;
	static INVERTERCOMMAND SetCommand= {0x0 , 0x0 , 0x0 , 0x0 , 0x0};		//? 给变频器发送的指令
	SetCommand.InverterAddress = DRIVE_ID;									//!	变频器ID, 这点需要注意,如果你的变频器ID不是1,则需要更改这个宏定义
	SetCommand.Command = 0X06;															//?	写命令	
	SetCommand.ParameterAddress = swap_uint16(0X2001);			//?	参数地址
	SetCommand.ParameterData = swap_uint16(setNewFrequency);//?	频率
	SetCommand.CrcCheck = crc16_modbus((uint8_t *)&SetCommand, 6);	//? 计算 CRC，传入前 6 字节（不包括 CrcCheck）
	if(xQueueSend(MotorRs485Queuesend , &SetCommand , NULL) != pdPASS){//?队列入队
		SEGGER_RTT_CustomFunction(1 , 0, "MotorRs485Queuesend is error  \r\n");
		nerror = -1;								
	}else{
		nerror = 0;
	}
	return nerror;
}


/**
 * @brief 处理频率收敛检查
 * @param setFreq 设置频率
 * @param runFreq 运行频率
 * @param threshold 阈值
 */
static void checkFrequencyConvergence(unsigned short setFreq, unsigned short runFreq, unsigned short threshold) {
    unsigned short difference = 0;
    if (setFreq > runFreq) {
        difference = setFreq - runFreq;
        if (difference > threshold) {
            if (xTimerIsTimerActive(SpeedTimers) == pdFALSE) {
                xTimerStart(SpeedTimers, TIMER_PERIOD);  // 启动定时器
            }
            if (g_SpeedTimeLoop >= TIMER_VPTSET) {
                xTimerStop(SpeedTimers, TIMER_PERIOD);
                SEGGER_RTT_CustomFunction(5, 0, "Stall error: timeout\r\n");
                xEventGroupClearBits(xCo_ChannelEventGroup, 0x01);
                xEventGroupSetBits(xCo_ChannelEventGroup, 0x02);  // 同频失败
            } else {
                SEGGER_RTT_CustomFunction(2, 0, "Frequency converging: loop = %d\r\n", g_SpeedTimeLoop);
            }
        } else if (difference <= threshold) {
            SEGGER_RTT_CustomFunction(2, 0, "Frequency converged: difference within threshold\r\n");
        }
    } else if (setFreq < runFreq) {
        difference = runFreq - setFreq;
        if (difference > threshold) {
            if (xTimerIsTimerActive(SpeedTimers) == pdFALSE) {
                xTimerStart(SpeedTimers, TIMER_PERIOD);  // 启动定时器
            }
            if (g_SpeedTimeLoop >= TIMER_VPTSET) {
                xTimerStop(SpeedTimers, TIMER_PERIOD);
                SEGGER_RTT_CustomFunction(5, 0, "Stall error: timeout\r\n");
                xEventGroupClearBits(xCo_ChannelEventGroup, 0x01);
                xEventGroupSetBits(xCo_ChannelEventGroup, 0x02);  // 同频失败
            } else {
                SEGGER_RTT_CustomFunction(2, 0, "Frequency converging: loop = %d\r\n", g_SpeedTimeLoop);
            }
        } else if (difference <= threshold) {
            SEGGER_RTT_CustomFunction(2, 0, "Frequency converged: difference within threshold\r\n");
            xEventGroupClearBits(xCo_ChannelEventGroup, 0x02);
            xEventGroupSetBits(xCo_ChannelEventGroup, 0x01);  // 同频完成
        }
    } else if (setFreq == runFreq) {
        SEGGER_RTT_CustomFunction(2, 0, "Frequencies equal: converged\r\n");
        xEventGroupClearBits(xCo_ChannelEventGroup, 0x02);
        xEventGroupSetBits(xCo_ChannelEventGroup, 0x01);  // 同频完成
    }
}



 /// @brief 同频细节部分 与队长的距离差≤1个节拍
 /// @param oneselfCycle 			自己的周期值ms
 /// @param oneselfWithCaptainDiff 	自己的周期与队长的周期差值
 /// @param CaptainFrequency 		队长的固定周期
 /// @return 
 unsigned short synchronizingFrequency(int oneselfCycle , int oneselfWithCaptainDiff , int CaptainFrequency)
 {
 	static int  Distance = 0; 	//? 	距离
 	int  Offset = 0 ; 	//? 	补偿值	
 	int deadTime = 5;			//?		失去时间
 	double scaleFactorK = 1.0;  //?		比例系数
 	static unsigned short setNewnGetCaptainSetFrequency = 0;	//? 给变频器输入新的频率值	
 	unsigned short nSetNewFrequency = 0;
 	if(oneselfWithCaptainDiff <= oneselfCycle * 1)
 	{
 		SEGGER_RTT_CustomFunction(5 , 0, "+++++++++++++++++++++++++++++++++ \r\n");
 		SEGGER_RTT_CustomFunction(5 , 0, "oneselfCycle / 2 = %d \r\n", oneselfCycle / 2);
 		SEGGER_RTT_CustomFunction(5 , 0, "oneselfWithCaptainDiff = %d \r\n", oneselfWithCaptainDiff);
 		if(oneselfWithCaptainDiff > (oneselfCycle / 2))
 		{
 			Distance = (oneselfWithCaptainDiff - oneselfCycle);
 			SEGGER_RTT_CustomFunction(5 , 0, "- Distance = %d \r\n", Distance);
 		}
 		else if(oneselfWithCaptainDiff < (oneselfCycle / 2))
 		{
 			Distance = oneselfWithCaptainDiff;
 			SEGGER_RTT_CustomFunction(5 , 0, "+ Distance = %d \r\n", Distance);
 		}	
 		else
 		{
 			SEGGER_RTT_CustomFunction(5 , 0, "oneselfWithCaptainDiff = Distance \r\n");
 		}
 		g_Distance = Distance;//! 实际差值全局
		
 		if(abs(oneselfWithCaptainDiff) > deadTime)//? 死区时间配置
 		{
 //			Offset = (double)(Distance * 0.25); 												//? 这里假设K系数为 0.5
 			Offset = (double)(Distance * 0.50); 												//? 这里假设K系数为 0.5
 			setNewnGetCaptainSetFrequency = CaptainFrequency + Offset;	//? 队长的基准频率 + 自己的补偿值
 			nSetNewFrequency = setNewnGetCaptainSetFrequency;
 			SEGGER_RTT_CustomFunction(5 , 0, "setNewnGetCaptainSetFrequency = %d \r\n", setNewnGetCaptainSetFrequency);
 			SEGGER_RTT_CustomFunction(5 , 0, "------------------------- \r\n");
 		}else{//? 无需补偿
 			SEGGER_RTT_CustomFunction(5 , 0, "the abs(oneselfWithCaptainDiff) = %d <= 10 \r\n", abs(oneselfWithCaptainDiff));
 			SEGGER_RTT_CustomFunction(5 , 0, "------------------------- \r\n");		
 		}
 	}
 	return nSetNewFrequency;
 }


#define MAX_TIME_LOOP 30
#define DIFF_THRESHOLD 100
#define INTERVAL_TIMEOUT 10000  // 10秒超时

static void handleMemberSync(unsigned char memberNum, short diff) 
{
    static unsigned timeLoop[4] = {0, 0, 0, 0};  // 成员同步计数器数组，初始值全为0
    if (memberNum >= 4) return;  // 超出范围，返回
    if (timeLoop[memberNum] <= MAX_TIME_LOOP) {
        timeLoop[memberNum]++;  // 计数增加，跟踪同步尝试次数
    } else if (timeLoop[memberNum] > MAX_TIME_LOOP) {
        if (CardId != memberNum) {  // 不是当前板卡，可能是其他成员
            timeLoop[memberNum] = 0;  // 重置计数器
            // TODO: 实现成员队列移除逻辑，例如：removeMemberFromQueue(memberNum);
        }
    }
    if (abs(diff) <= DIFF_THRESHOLD) {  // 差值小于阈值，同步完成
        timeLoop[memberNum] = 0;  // 重置计数器
    }
    // 显示逻辑：负差值显示为负，正差值显示为正
    if (diff < 0) {
			// 由于是LCD屏幕 , 需要 补码转换
        displayQueueMessage(memberNum, 1, abs(diff) / 100);  // 负差值
    } else {
        displayQueueMessage(memberNum, 0, diff / 100);  // 正差值
    }
}

#define CO_CHANNEL_COMPLETE 0x01
#define CO_CHANNEL_FAIL 0x02
#define TIMER_PERIOD pdMS_TO_TICKS(1000)
#define TIMER_VPTSET 17500

/**
 * @brief 检查频率同步状态并处理超时
 * @param setFreq 设置频率
 * @param runFreq 运行频率
 * @param threshold 频率差值阈值
 */
static void checkFrequencySynchronization(unsigned short setFreq, unsigned short runFreq, unsigned short threshold) {
    int difference = (int)setFreq - (int)runFreq;  // 计算差值，考虑符号
    unsigned short absDifference = abs(difference);  // 取绝对值

    if (absDifference <= threshold) {
        // 频率差值在阈值内，同步完成
        if (xTimerIsTimerActive(SpeedTimers) == pdTRUE) {
            xTimerStop(SpeedTimers, 0);  // 停止定时器
        }
        SEGGER_RTT_CustomFunction(2, 0, "Co_ChannelBits is complete\r\n");
        xEventGroupClearBits(xCo_ChannelEventGroup, CO_CHANNEL_FAIL);
        xEventGroupSetBits(xCo_ChannelEventGroup, CO_CHANNEL_COMPLETE);
    } else {
        // 频率差值超过阈值，启动定时器
        if (xTimerIsTimerActive(SpeedTimers) == pdFALSE) {
            xTimerStart(SpeedTimers, TIMER_PERIOD);  // 启动定时器
        }
        // 检查定时器是否超时
        if (g_SpeedTimeLoop >= TIMER_VPTSET) {
            xTimerStop(SpeedTimers, TIMER_PERIOD);
            SEGGER_RTT_CustomFunction(5, 0, "stall is error\r\n");  // 记录失速错误
            xEventGroupClearBits(xCo_ChannelEventGroup, CO_CHANNEL_COMPLETE);
            xEventGroupSetBits(xCo_ChannelEventGroup, CO_CHANNEL_FAIL);  // 设置同频失败
        } else {
            SEGGER_RTT_CustomFunction(2, 0, "g_SpeedTimeLoop = %d\r\n", g_SpeedTimeLoop);  // 记录同步过程中计时
        }
    }
}

 /// @brief Segger 窗口  2 - 5 , 5 错误提示
 /// @param argument 
  void SyncLogicTask(void *argument)
  {
  	EventBits_t SyncBits; 																//? 同步
  	EventBits_t Co_ChannelBits; 													//? 同频
  	EventBits_t CaptainExistBits; 												//? 队长存在
  	EventBits_t CaptainOwnBits; 													//? 自己是不是队长
  	EventBits_t PulseBits;																//? 脉冲有效标志
  	const TickType_t xTicksToWait = portMAX_DELAY; 				// 阻塞等待，提高响应性	
  	unsigned short nGetSetFrequency = 0; 									//? 任务中的设定频率
  	unsigned short nGetRunFrequency = 0; 									//? 任务中的运行频率	
  	unsigned short nGetCaptainSetFrequency = 0; 					//? 队长运行的频率
  	unsigned short Difference = 0;												//? 差值计算
  	unsigned short Co_ChannelBitsVPT = 10;								//? 运行频率与实际频率 阈值
  	unsigned short CaptainCo_ChannelBitsVPT = 10;					//? 运行频率与实际频率 阈值
  	INVERTERCOMMAND SetCommand= {0x0 , 0x0 , 0x0 , 0x0 , 0x0};		//? 给变频器发送的指令
  	BOTTLING SetBottling = {0 , 0 , 0};								//? 设置制瓶子机队列
  	BOTTLING SpyOn = {0 , 0, 0};									//? 窥视制瓶子机队列
  	BOTTLING TakeOut = {0 , 0, 0};									//? 取出制瓶子机队列
  	unsigned char  CanSyncData[8] = {0};
  	unsigned short CanSyncID = 0X00;
  //	CanTxHeadType Bottling;
  	LCDShowQueue member;
  	while(1)
  	{
  		PulseBits = xEventGroupWaitBits(xPulseEventGroup, // 事件标志组句柄
  									 0x01,               								// 等待bit0和bit1被设置
  									 pdTRUE,            								// 退出时清除等待的位
  									 pdFALSE,           								// 任一位被置位即返回
  									 xTicksToWait); 	 									// 等待延迟时间
  		if(PulseBits == 1)
  		{
  			SyncBits = xEventGroupGetBits(xSyncEventGroup); //? 同步模式标志			
  			if(SyncBits & 0x01)//?同步模式
  			{
  				Co_ChannelBits 		= xEventGroupGetBits(xCo_ChannelEventGroup);			//? 读取同频标志
  				CaptainExistBits 	= xEventGroupGetBits(xCaptainExistEventGroup);		//? 是否存在队长标志判断
  				xSemaphoreTake(xMutex, portMAX_DELAY); 															//! 不管是否同频,都应该去更新
  				nGetSetFrequency = g_SetFrequencyUpdateValue;												//? 更新设置频率
  				nGetRunFrequency = g_RunFrequencyUpdateValue;												//? 更新运行频率
  				nGetCaptainSetFrequency = g_CaptainSetFrequencyUpdateValue;					//? 更新队长的频率
  				xSemaphoreGive(xMutex);					
					if (Co_ChannelBits == 0) {
						if (CaptainExistBits == 1) {
							adjustFrequencyToCaptain(nGetSetFrequency, nGetCaptainSetFrequency, CaptainCo_ChannelBitsVPT);
						}
						// 调用优化后的函数
						checkFrequencySynchronization(nGetSetFrequency, nGetRunFrequency, Co_ChannelBitsVPT);
					}				
  				else if(Co_ChannelBits == 1)//? 同频完成
  				{
  					if(CaptainExistBits == 0)
  					{				
  						SetBottling.CardNum = CardId;
  						SetBottling.FactFrequency = nGetSetFrequency;
  						SetBottling.CaptainSign = 1;													//?	设置自己为队长
							displayLeader(SetBottling.CaptainSign);
  						xEventGroupSetBits(xCaptainExistEventGroup, 0x01);		//? 设置系统标志位 ， 否则 自己是不是队长不清楚
  						if(xQueueSend(BottlingQueuesend , &SetBottling , NULL) != pdPASS){//?队列入队
  							SEGGER_RTT_CustomFunction(5 , 0, "BottlingQueuesend is error  \r\n");
								SetBottling.MemberWithCaptainDiffe = 0;	
  						}	
  					}
  					if(SetBottling.CaptainSign == 0){
							displayLeader(SetBottling.CaptainSign);		
						}
						if (CardId > 3){
						}
						else{
							SEGGER_RTT_CustomFunction(6 , 0, "g_CaptainSyncDIFISR = %d  \r\n" , g_CaptainSyncDIFISR);
							CanSyncMessage(CardId , 0x1234 , nGetSetFrequency , nGetRunFrequency , g_CaptainSyncDIFISR , SetBottling.CaptainSign , 0 , 1);	
						}				
  					// 查看队列中的数据但不取出
  					if (xQueuePeek(BottlingQueuesend, &SpyOn, pdMS_TO_TICKS(500)) == pdPASS) {			
  					}
  					//? 是不是队长 , 窥视队列数据
  					//if(SpyOn.CaptainSign == 1)
						if (SetBottling.CaptainSign == 1) {  // 当前为队长
							BOTTLING SpyOn;
							if (xQueuePeek(BottlingQueuesend, &SpyOn, pdMS_TO_TICKS(500)) == pdPASS) {  // 检查队列数据，500ms超时
								if (g_interval_ms > INTERVAL_TIMEOUT) {  // 超时，放弃队长身份
									BOTTLING TakeOut;
									xQueueReceive(BottlingQueuesend, &TakeOut, 0);  // 从队列移除数据
									// TODO: 可能需要设置 CaptainSign 为 0 或其他处理
								} else {  // 未超时，处理成员同步
									LCDShowQueue member;
									if (xQueueReceive(MemberMessag, &member, pdMS_TO_TICKS(10)) == pdPASS) {  // 接收成员消息，10ms超时
										handleMemberSync(member.memberNum, member.memberWithCaptainDiff);  // 处理成员同步逻辑
									} else {
										SEGGER_RTT_CustomFunction(5, 0, "MemberMessag read error\r\n");  // 记录读取错误
									}
								}
							} else {
								SEGGER_RTT_CustomFunction(5, 0, "BottlingQueuesend peek error\r\n");  // 记录队列检查错误
							}
						}					
  					else if(SetBottling.CaptainSign == 0)
  					{
  						//* ++++++++++++++++++++	Begin 队员 去 追赶队长操作  ++++++++++++++++++++
  						static unsigned short setNewFrequency = 0;
  						setNewFrequency = synchronizingFrequency(g_interval_ms , g_CaptainSyncDIFISR , nGetCaptainSetFrequency);
  						if(setNewFrequency != 0)
  						{
  							if(UivertorRefreshData(setNewFrequency) == 0)
  							{
  								SEGGER_RTT_CustomFunction(5 , 0,"setNewFrequency = %d \r\n",setNewFrequency);
  								xEventGroupSetBits(xFrequencyUpdateLCDCaptain, 0x01);//? 在LCD上更新频率
  							}							
  						}						
  						else if(g_CaptainSyncDIFISR > g_interval_ms * 1)
  						{	//? 队长触发到自己触发的差值 大于 自己的节拍*5
  							//? 赶不上了,没救了 , 自己不是队长
  							SEGGER_RTT_CustomFunction(5 , 0, RTT_CTRL_TEXT_BRIGHT_RED"Excessive deviation value \r\n");
  							//? 队列迁移????????
  							//? 自己是不是队长
  							//? 是队长,装订自己的配速
  						}
  						//? LCD显示差距
  						if(g_Distance >= 0)
  						{
								displayQueueMemberMessage(CardId, 0, abs(g_Distance) / 100);
  						}
  						else if(g_Distance < 0)
  						{
								displayQueueMemberMessage(CardId, 1, abs(g_Distance) / 100);						
  						}				
  						//* -------------------- end 队员 去 追赶队长操作  --------------------
  					}
  					else{//? 未知错误
  						SEGGER_RTT_CustomFunction(5 , 0, RTT_CTRL_TEXT_BRIGHT_RED"unknown error \r\n");
  					}		
  				}					
  				else if(Co_ChannelBits == 2){//? 同频失败 , 失速报警
  					SEGGER_RTT_CustomFunction(5, 0 ,RTT_CTRL_TEXT_BRIGHT_YELLOW"co-Channel fail Stall alarm \r\n");
  				}
  				else{//? 同频失败 , 未知错误
  					SEGGER_RTT_CustomFunction(5, 0 ,RTT_CTRL_TEXT_BRIGHT_RED"co-Channel fail unknown error \r\n");
  				}
  			}
  			else if(SyncBits & 0x02)//?非同步模式
  			{	//* 非同步模式, 
  				//? 同频标志位置0
  				//? 调速计数器清零
  				//? 需要发送CAN数据表示自己放弃队长
  				SetBottling.CaptainSign = 0;
 				if (CardId > 3){
 				}
 				else{
 					CanSyncMessage(CardId , 0x1234 , nGetSetFrequency , nGetRunFrequency , g_Distance , SetBottling.CaptainSign , 0 , 0);	
 				}
				xEventGroupClearBits(xCaptainExistEventGroup, 0x01);	
				xEventGroupClearBits(xCo_ChannelEventGroup, 0x01);
				xEventGroupClearBits(xCo_ChannelEventGroup, 0x02);
				SEGGER_RTT_CustomFunction(5, 0 ," \r\n");
  			}
  		}
  	}
  }
